#include "ctrl.h"
#include "motor_pwm.h"
#include "led.h"
#include "config.h"
#include "adc.h"
#include "math.h"
#include "AS5600.h"
#include "stdlib.h"

#include "measuretask.h"
#include "keytask.h"

#include "fast_calculate.h"
#include "foc_motor.h"
#include "PID.h"
/*FreeRtos includes*/
#include "FreeRTOS.h"
#include "task.h"
#include "hw_config.h"
//////////////////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////////////////

PidObject _sPidAngle;
PidObject _sPidVelocity;

void v_motor_init(void)
{
    configParam._pMotorConfig.iMotionControlType = Type_velocity;
    configParam._pMotorConfig.fVoltageLimit = 0.9;
    configParam._pMotorConfig.fVelocityLimit = 1;

    _sMotorConfig.iMotionControlType = Type_velocity;
    _sMotorConfig.fVoltageLimit = 0.9;
    _sMotorConfig.fVelocityLimit = 1;

    TIM2_PWM_Init();
    v_ctrl_pin_config();
    b_rectify_senser();
    v_pid_init();
}


void v_pid_init(void)
{
    _sPidAngle.deriv = 0;
    _sPidAngle.desired = 0;
    _sPidAngle.error = 0;
    _sPidAngle.integ = 0;
    _sPidAngle.IntegLimitHigh = _sMotorConfig.fVelocityLimit;
    _sPidAngle.IntegLimitLow = -_sMotorConfig.fVelocityLimit;
    _sPidAngle.kp = 1.50f;
    _sPidAngle.ki = 0;
    _sPidAngle.kd = 0;
    _sPidAngle.measured = 0;
    _sPidAngle.offset = 0;
    _sPidAngle.out = 0;
    _sPidAngle.OutLimitHigh = _sMotorConfig.fVelocityLimit;
    _sPidAngle.OutLimitLow = -_sMotorConfig.fVelocityLimit;
    _sPidAngle.prevError = 0;

    _sPidVelocity.deriv = 0;
    _sPidVelocity.desired = 0;
    _sPidVelocity.error = 0;
    _sPidVelocity.integ = 0;
    _sPidVelocity.IntegLimitHigh = 3;
    _sPidVelocity.IntegLimitLow = -3;
    _sPidVelocity.kp = 4.0f;
    _sPidVelocity.ki = 0.1f;
    _sPidVelocity.kd = 0.0f;
    _sPidVelocity.measured = 0;
    _sPidVelocity.offset = 0;
    _sPidVelocity.out = 0;
    _sPidVelocity.OutLimitHigh = _sMotorConfig.fVoltageLimit;
    _sPidVelocity.OutLimitLow = -_sMotorConfig.fVoltageLimit;
    _sPidVelocity.prevError = 0;
}

#define REDUCE (-0.4f)
#define INCRECE (0.4f)


u8 ucModle = VOLUM;

u8 uc_get_ucModle(void)
{
    return ucModle;
}

void v_control_task(void *pvParameters)
{
    uint8_t buff[9] = {0};
    uint8_t ucBuffLen = 2;
    
    float fNewTarget;
    float fCurrentSet;
    float fLastAngle;
    
    fNewTarget= f_get_attitude_angle();
    fLastAngle = fNewTarget;
    
    
    while(1)
    {
        _sPidAngle.measured = f_get_attitude_angle();
        _sPidVelocity.measured = f_get_attitude_speed();

        switch(ucModle)
        {
            case VOLUM:
                ucBuffLen = 2;
                v_set_motor_state(true);
                _sPidVelocity.desired = 0;
                _sVoltage.q = 0;
                if((_sPidAngle.measured - fLastAngle) > 0.2f)
                {
                    buff[0] = 0x02;
                    buff[1] = 0x01;

                    fLastAngle = _sPidAngle.measured;
                }
                else if((_sPidAngle.measured - fLastAngle) < -0.2f)
                {
                    buff[0] = 0x02;
                    buff[1] = 0x02;

                    fLastAngle = _sPidAngle.measured;
                }
                else
                {
                    buff[0] = 0x02;
                    buff[1] = 0x00;

                }
                break;

            case KICK:
                ucBuffLen = 2;
                v_set_motor_state(true);
            
                if((_sPidAngle.measured - fLastAngle) > INCRECE)
                {
                    fNewTarget = fLastAngle + INCRECE * 1.5f;
                    fLastAngle = fNewTarget;
                    _sPidVelocity.integ = 0;
                    
                    buff[0] = 0x02;
                    buff[1] = 0x20;
                }
                else if((_sPidAngle.measured - fLastAngle) < REDUCE)
                {
                    fNewTarget = fLastAngle + REDUCE * 1.5f;
                    fLastAngle = fNewTarget;
                    _sPidVelocity.integ = 0;
                    
                    buff[0] = 0x02;
                    buff[1] = 0x40;
                }
                else
                {
                    buff[0] = 0x02;
                    buff[1] = 0x00;
                }

                // angle set point
                _sPidAngle.desired = fNewTarget;
                // calculate velocity set point
                v_pid_update(&_sPidAngle);
                _sPidVelocity.desired = _sPidAngle.out;
                // calculate the torque command
                v_pid_update(&_sPidVelocity);
                fCurrentSet = _sPidVelocity.out;
                // if torque controlled through voltage
                if(_sMotorConfig.iTorqueControlType == Type_voltage)
                {
                    _sVoltage.q = fCurrentSet;
                    _sVoltage.d = 0;
                }
                break;

            case WHEEL:
                ucBuffLen = 5;
                v_set_motor_state(true);
                
                if((_sPidAngle.measured - fLastAngle) > INCRECE)
                {
                    fNewTarget = fLastAngle + INCRECE * 1.5f;
                    fLastAngle = fNewTarget;
                    _sPidVelocity.integ = 0;
                    
                    buff[0] = 0x03;
                    buff[1] = 0x00;
                    buff[2] = 0x00;
                    buff[3] = 0x00;
                    buff[4] = 1;
                }
                else if((_sPidAngle.measured - fLastAngle) < REDUCE)
                {
                    fNewTarget = fLastAngle + REDUCE * 1.5f;
                    fLastAngle = fNewTarget;
                    _sPidVelocity.integ = 0;
                    
                    buff[0] = 0x03;
                    buff[1] = 0x00;
                    buff[2] = 0x00;
                    buff[3] = 0x00;
                    buff[4] = 0x00;
                }
                else
                {
                    buff[0] = 0x03;
                    buff[1] = 0x00;
                    buff[2] = 0x00;
                    buff[3] = 0x00;
                    buff[4] = 0x00;
                }

                // angle set point
                _sPidAngle.desired = fNewTarget;
                // calculate velocity set point
                v_pid_update(&_sPidAngle);
                _sPidVelocity.desired = _sPidAngle.out;
                // calculate the torque command
                v_pid_update(&_sPidVelocity);
                fCurrentSet = _sPidVelocity.out;
                // if torque controlled through voltage
                if(_sMotorConfig.iTorqueControlType == Type_voltage)
                {
                    _sVoltage.q = fCurrentSet;
                    _sVoltage.d = 0;
                }
            break;
            
            case ROLL:
                ucBuffLen = 2;
                v_set_motor_state(true);

                _sPidVelocity.desired = 0.05;//fNewTarget;
                // calculate the torque command
                v_pid_update(&_sPidVelocity); // if current/foc_current torque control
                fCurrentSet = _sPidVelocity.out;
                // if torque controlled through voltage control
                if(_sMotorConfig.iTorqueControlType == Type_voltage)
                {
                    _sVoltage.q = fCurrentSet;
                    // use voltage if phase-resistance not provided
                    _sVoltage.d = 0;
                }
                break;
            
            default: break;
        }
        
        u8 ucKeyState = ucGetKeyState();
        if(KEY_SHORT_PRESS == ucKeyState)
        {
            ucModle++;
            if(ucModle > ROLL)
            {
                ucModle = 0;
            }
            fLastAngle = _sPidAngle.measured;
            fNewTarget = _sPidAngle.measured;
        }
        else if(KEY_LONG_PRESS == ucKeyState)
        {
            ucBuffLen = 2;
            buff[0] = 0x02;
            buff[1] = 0x08;
        }
        
        HID_Send(buff,ucBuffLen);
  
        float fElectricalAngle = f_normalize_angle(_sPidAngle.measured * _sMotorConfig.uiPolePairs - _sDynamicParam.fZeroElectricAngle);

        setPhaseVoltage(_sVoltage.q, _sVoltage.d, fElectricalAngle);

        vTaskDelay(1);
    }
}

